/**
 * @file /src/qnode.cpp
 *
 * @brief Ros communication central!
 *
 * @date February 2011
 **/

/*****************************************************************************
** Includes
*****************************************************************************/

#include <ros/ros.h>
#include <ros/network.h>
#include <string>
#include <sstream>
#include "../include/car_sim/qnode.hpp"
#include "math.h"
#include <QMetaType>

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace car_sim {

/*****************************************************************************
** Implementation
*****************************************************************************/

QNode::QNode(int argc, char** argv ) :
	init_argc(argc),
	init_argv(argv)
	{}

QNode::~QNode() {
    if(ros::isStarted()) {
      ros::shutdown(); // explicitly needed since we use ros::start();
      ros::waitForShutdown();
    }
	wait();
}

bool QNode::init() {
	ros::init(init_argc,init_argv,"car_sim");
	if ( ! ros::master::check() ) {
		return false;
	}
	ros::start(); // explicitly needed since our nodehandle is going out of scope.
	ros::NodeHandle n;
	// Add your ros communications here.
	chatter_publisher = n.advertise<std_msgs::String>("chatter", 1000);

    /******************************************************************************************************************************************************
     * My Code Below ***************************************************************************************************************************************
     * ******************************************************************************************************************************************************/

    qRegisterMetaType<QVector<int>>("QVector<int>");
    //hello world订阅者
    chatter_sub = n.subscribe("chatter", 1000, &QNode::chatter_callback, this);
    //运动信息发布者
    cmd_vel_pub = n.advertise<geometry_msgs::Twist>("/smart/cmd_vel", 1000);
    //里程计订阅者
    odom_sub = n.subscribe("raw_odom", 1000, &QNode::odom_callback, this);

    /******************************************************************************************************************************************************
     * My Code Above ---------------------------------------------------------------------------------------------------------------------------------------
     * ******************************************************************************************************************************************************/

	start();
	return true;
}

bool QNode::init(const std::string &master_url, const std::string &host_url) {
	std::map<std::string,std::string> remappings;
	remappings["__master"] = master_url;
	remappings["__hostname"] = host_url;
	ros::init(remappings,"car_sim");
	if ( ! ros::master::check() ) {
		return false;
	}
	ros::start(); // explicitly needed since our nodehandle is going out of scope.
	ros::NodeHandle n;
	// Add your ros communications here.
	chatter_publisher = n.advertise<std_msgs::String>("chatter", 1000);

    /******************************************************************************************************************************************************
     * My Code Below ***************************************************************************************************************************************
     * ******************************************************************************************************************************************************/

    qRegisterMetaType<QVector<int>>("QVector<int>");
    //hello world订阅者
    chatter_sub = n.subscribe("chatter", 1000, &QNode::chatter_callback, this);
    //运动信息发布者
    cmd_vel_pub = n.advertise<geometry_msgs::Twist>("/smart/cmd_vel", 1000);
    //里程计订阅者
    odom_sub = n.subscribe("raw_odom", 1000, &QNode::odom_callback, this);

    /******************************************************************************************************************************************************
     * My Code Above ---------------------------------------------------------------------------------------------------------------------------------------
     * ******************************************************************************************************************************************************/

	start();
	return true;
}

/******************************************************************************************************************************************************
 * My Code Below ***************************************************************************************************************************************
 * ******************************************************************************************************************************************************/

void QNode::chatter_callback(const std_msgs::String &msg){
    log(Debug, "I receive" + msg.data);
}

void QNode::set_cmd_vel(char k, float linear, float angular){
    std::map<char, std::vector<float>> moveBindings
    {
        {'I', {1, 0}},
        {'K', {-1, 0}},
        {'J', {0, 1}},
        {'L', {0, -1}}
    };
    char key = k;
    this->x = moveBindings[key][0];
    this->z = moveBindings[key][1];

    geometry_msgs::Twist twist;
    twist.linear.x = this->x * linear;
    twist.angular.z = this->z * angular;

    this->cmd_vel_pub.publish(twist);
}

void QNode::sent_cmd_vel(char k){
    std::map<char, std::vector<float>> moveBindings
    {
        {'W', {1, 0}},
        {'S', {-1, 0}},
        {'A', {0, 1}},
        {'D', {0, -1}}
    };
    char key = k;
    this->x = this->x + moveBindings[key][0] * this->acc * 0.1;
    this->z = this->z + moveBindings[key][1] * this->turn_acc * 0.1;
    if(this->x >= this->x_max) this->x = this->x_max;
    if(this->x <= -this->x_max) this->x = -this->x_max;
    if(this->z >= this->z_max) this->z = this->z_max;
    if(this->z <= -this->z_max) this->z = -this->z_max;
}

void QNode::odom_callback(const nav_msgs::Odometry &msg){
    emit speed_vel(msg.twist.twist.linear.x, msg.twist.twist.linear.y);
}

/******************************************************************************************************************************************************
 * My Code Above ---------------------------------------------------------------------------------------------------------------------------------------
 * ******************************************************************************************************************************************************/

void QNode::run() {
    ros::Rate loop_rate(50);
	int count = 0;
	while ( ros::ok() ) {

        /******************************************************************************************************************************************************
         * My Code Below ***************************************************************************************************************************************
         * ******************************************************************************************************************************************************/

        if(this->keyboard_press['W']){
            this->x = this->x + this->acc * 0.1;
            if(this->x >= this->x_max) this->x = this->x_max;
            if(this->x <= -this->x_max) this->x = -this->x_max;
        }else if(keyboard_press['S']){
            this->x = this->x - this->acc * 0.1;
            if(this->x >= this->x_max) this->x = this->x_max;
            if(this->x <= -this->x_max) this->x = -this->x_max;
        }else{
            if(this->x > 0){
                this->x = this->x - this->acc * 0.1;
            }else{
                this->x = this->x + this->acc * 0.1;
            }
            if(fabs(this->x) <= this->acc * 0.1){
                this->x = 0;
            }
        }

        if(this->keyboard_press['A']){
            this->z = this->z + this->turn_acc * 0.01;
            if(this->z >= this->z_max) this->z = this->z_max;
            if(this->z <= -this->z_max) this->z = -this->z_max;
        }else if(this->keyboard_press['D']){
            this->z = this->z - this->turn_acc * 0.01;
            if(this->z >= this->z_max) this->z = this->z_max;
            if(this->z <= -this->z_max) this->z = -this->z_max;
        }else{
            if(this->z > 0){
                this->z = this->z - this->turn_acc * 0.01;
            }else{
                this->z = this->z + this->turn_acc * 0.01;
            }
            if(fabs(this->z) <= this->turn_acc * 0.01){
                this->z = 0;
            }
        }

        if(this->ptr_progressBar_acc != nullptr){
            this->ptr_progressBar_acc->setValue((this->x * 10) + 100);
        }
        if(this->ptr_progressBar_turning != nullptr){
            this->ptr_progressBar_turning->setValue((-this->z * 100) + 50);
        }

        geometry_msgs::Twist twist;
        twist.linear.x = this->x;
        twist.angular.z = this->z;
        this->cmd_vel_pub.publish(twist);

        /******************************************************************************************************************************************************
         * My Code Above ---------------------------------------------------------------------------------------------------------------------------------------
         * ******************************************************************************************************************************************************/

        /*
		std_msgs::String msg;
		std::stringstream ss;
		ss << "hello world " << count;
		msg.data = ss.str();
		chatter_publisher.publish(msg);
		log(Info,std::string("I sent: ")+msg.data);
        ++count;
        */

		ros::spinOnce();
		loop_rate.sleep();	
	}
	std::cout << "Ros shutdown, proceeding to close the gui." << std::endl;
	Q_EMIT rosShutdown(); // used to signal the gui for a shutdown (useful to roslaunch)
}


void QNode::log( const LogLevel &level, const std::string &msg) {
	logging_model.insertRows(logging_model.rowCount(),1);
	std::stringstream logging_model_msg;
	switch ( level ) {
		case(Debug) : {
				ROS_DEBUG_STREAM(msg);
				logging_model_msg << "[DEBUG] [" << ros::Time::now() << "]: " << msg;
				break;
		}
		case(Info) : {
				ROS_INFO_STREAM(msg);
				logging_model_msg << "[INFO] [" << ros::Time::now() << "]: " << msg;
				break;
		}
		case(Warn) : {
				ROS_WARN_STREAM(msg);
				logging_model_msg << "[INFO] [" << ros::Time::now() << "]: " << msg;
				break;
		}
		case(Error) : {
				ROS_ERROR_STREAM(msg);
				logging_model_msg << "[ERROR] [" << ros::Time::now() << "]: " << msg;
				break;
		}
		case(Fatal) : {
				ROS_FATAL_STREAM(msg);
				logging_model_msg << "[FATAL] [" << ros::Time::now() << "]: " << msg;
				break;
		}
	}
	QVariant new_row(QString(logging_model_msg.str().c_str()));
	logging_model.setData(logging_model.index(logging_model.rowCount()-1),new_row);
	Q_EMIT loggingUpdated(); // used to readjust the scrollbar
}

}  // namespace car_sim
